// --------------------------------------------------------------------------------------------------------------------
// <copyright file="QuaternionTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Numerics
{
    using System;

    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Quaternion test class
    /// </summary>
    [TestClass]
    public class QuaternionTest
    {
        /// <summary>
        /// Test default constructor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionDefaultConstructor()
        {
            Quaternion quat = new Quaternion();
            Assert.IsTrue(
                quat.X == 0.0 && quat.Y == 0.0 && quat.Z == 0.0 && quat.W == 0.0, 
                "Quaternion Components are not zero in default constructor");
        }

        /// <summary>
        /// Test constructor with doubles
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionDoubleConstructor()
        {
            Quaternion quat = new Quaternion(1.0, 2.0, 3.0, 4.0);
            Assert.IsTrue(
                quat.X == 1.0 && quat.Y == 2.0 && quat.Z == 3.0 && quat.W == 4.0, 
                "Quaternion Components are not what was passed into constructor");
        }

        /// <summary>
        /// Test copy constructor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionCopyConstructor()
        {
            Quaternion initialQuaternion = new Quaternion(1.0, 2.0, 3.0, 4.0);
            Quaternion v = new Quaternion(initialQuaternion);
            Assert.IsTrue(v.X == 1.0 && v.Y == 2.0 && v.Z == 3.0 && v.W == 4.0, "Quaternion copy constructor failed");
        }

        /// <summary>
        /// Test homogenous transform constructor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionHomogenousTransformConstructor()
        {
            Matrix4 originalTransform = HomogenousTransformTest.GetRandomHomogenousTransform();
            Quaternion quat = new Quaternion(originalTransform);
            Matrix4 convertedTransform = HomogenousTransform.CreateFromQuaternion(quat);

            for (int i = 0; i < 4; ++i)
            {
                // not checking "position" column
                for (int j = 0; j < 3; ++j)
                {
                    Assert.IsTrue(
                        Math.Abs(originalTransform[i, j] - convertedTransform[i, j]) < MathConstants.ErrorEpsilon,
                        "Did not convert properly from homogenous transform to quaternion and back");
                }
            }
        }

        /// <summary>
        /// Test multiply operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionMultiplyOperator()
        {
            Quaternion leftSide = new Quaternion();
            Quaternion rightSide = new Quaternion();

            leftSide.X = 0.0;
            leftSide.Y = 0.0;
            leftSide.Z = 0.0;
            leftSide.W = 2.0;
            rightSide.X = 0.0;
            rightSide.Y = 0.0;
            rightSide.Z = 0.0;
            rightSide.W = 2.0;

            Quaternion result = leftSide * rightSide;
            Assert.IsTrue(
                Math.Abs(result.W - 4.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion multiply W only failed result = " + result.ToString()));

            leftSide.W = 0.0;
            leftSide.X = 2.0;
            rightSide.W = 0.0;
            rightSide.X = 2.0;
            result = leftSide * rightSide;
            Assert.IsTrue(
                Math.Abs(result.W + 4.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion multiply X only failed result = " + result.ToString()));

            leftSide.X = 0.0;
            leftSide.Y = 2.0;
            rightSide.X = 0.0;
            rightSide.Y = 2.0;
            result = leftSide * rightSide;
            Assert.IsTrue(
                Math.Abs(result.W + 4.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion multiply Y only failed result = " + result.ToString()));

            leftSide.Y = 0.0;
            leftSide.Z = 2.0;
            rightSide.Y = 0.0;
            rightSide.Z = 2.0;
            result = leftSide * rightSide;
            Assert.IsTrue(
                Math.Abs(result.W + 4.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion multiply Z only failed result = " + result.ToString()));

            leftSide.Z = 0.0;
            leftSide.W = 2.0;
            rightSide.Z = 0.0;
            rightSide.X = 2.0;
            result = leftSide * rightSide;
            Assert.IsTrue(
                Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 4.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion multiply W-X failed result = " + result.ToString()));

            leftSide.W = 0.0;
            leftSide.X = 2.0;
            rightSide.X = 0.0;
            rightSide.W = 2.0;
            result = leftSide * rightSide;
            Assert.IsTrue(
                Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 4.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion multiply X-W failed result = " + result.ToString()));

            leftSide.X = 0.0;
            leftSide.Y = 2.0;
            rightSide.W = 0.0;
            rightSide.Z = 2.0;
            result = leftSide * rightSide;
            Assert.IsTrue(
                Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 4.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion multiply Y-Z failed result = " + result.ToString()));

            leftSide.Y = 0.0;
            leftSide.Z = 2.0;
            rightSide.Z = 0.0;
            rightSide.Y = 2.0;
            result = leftSide * rightSide;
            Assert.IsTrue(
                Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X + 4.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion multiply Z-Y failed result = " + result.ToString()));

            leftSide.Z = 0.0;
            leftSide.X = 2.0;
            rightSide.Y = 0.0;
            rightSide.Z = 2.0;
            result = leftSide * rightSide;
            Assert.IsTrue(
                Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y + 4.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion multiply X-Z failed result = " + result.ToString()));

            leftSide.X = 0.0;
            leftSide.Y = 2.0;
            rightSide.Z = 0.0;
            rightSide.W = 2.0;
            result = leftSide * rightSide;
            Assert.IsTrue(
                Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 4.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion multiply Y-W failed result = " + result.ToString()));

            leftSide.Y = 0.0;
            leftSide.Z = 2.0;
            rightSide.W = 0.0;
            rightSide.X = 2.0;
            result = leftSide * rightSide;
            Assert.IsTrue(
                Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 4.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion multiply Z-X failed result = " + result.ToString()));

            leftSide.Z = 0.0;
            leftSide.W = 2.0;
            rightSide.X = 0.0;
            rightSide.Z = 2.0;
            result = leftSide * rightSide;
            Assert.IsTrue(
                Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 4.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion multiply W-Z failed result = " + result.ToString()));

            leftSide.W = 0.0;
            leftSide.X = 2.0;
            rightSide.Z = 0.0;
            rightSide.Y = 2.0;
            result = leftSide * rightSide;
            Assert.IsTrue(
                Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 4.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion multiply X-Y failed result = " + result.ToString()));

            leftSide.X = 0.0;
            leftSide.Y = 2.0;
            rightSide.Y = 0.0;
            rightSide.X = 2.0;
            result = leftSide * rightSide;
            Assert.IsTrue(
                Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z + 4.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion multiply Y-X failed result = " + result.ToString()));

            leftSide.Y = 0.0;
            leftSide.Z = 2.0;
            rightSide.X = 0.0;
            rightSide.W = 2.0;
            result = leftSide * rightSide;
            Assert.IsTrue(
                Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 4.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion multiply Z-W failed result = " + result.ToString()));
        }

        /// <summary>
        /// Test inverse method
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionInverse()
        {
            Quaternion quat = new Quaternion();
            quat.X = 2.0;
            Quaternion result = Quaternion.Inverse(quat);
            Assert.IsTrue(
                Math.Abs(result.X + (1.0 / quat.X)) < MathConstants.ErrorEpsilon && result.Y == 0.0 && result.Z == 0.0
                && result.W == 0.0, 
                string.Format("Quaternion inverse X only method failed result = " + result.ToString()));

            quat.X = 0.0;
            quat.Y = 2.0;
            result = Quaternion.Inverse(quat);
            Assert.IsTrue(
                result.X == 0.0 && Math.Abs(result.Y + (1.0 / quat.Y)) < MathConstants.ErrorEpsilon && result.Z == 0.0
                && result.W == 0.0, 
                string.Format("Quaternion inverse Y only method failed result = " + result.ToString()));

            quat.Y = 0.0;
            quat.Z = 2.0;
            result = Quaternion.Inverse(quat);
            Assert.IsTrue(
                result.X == 0.0 && result.Y == 0.0 && Math.Abs(result.Z + (1.0 / quat.Z)) < MathConstants.ErrorEpsilon
                && result.W == 0.0, 
                string.Format("Quaternion inverse Z only method failed result = " + result.ToString()));

            quat.Z = 0.0;
            quat.W = 2.0;
            result = Quaternion.Inverse(quat);
            Assert.IsTrue(
                result.X == 0.0 && result.Y == 0.0 && result.Z == 0.0
                && Math.Abs(result.W - (1.0 / quat.W)) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion inverse W only method failed result = " + result.ToString()));
        }

        /// <summary>
        /// Test subtraction operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionSubtractOperator()
        {
            Quaternion leftSide = new Quaternion(1.0, 2.0, 3.0, 4.0);
            Quaternion rightSide = new Quaternion(1.0, 2.0, 3.0, 4.0);
            Quaternion result = leftSide - rightSide;
            Assert.IsTrue(
                Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion subtraction failed result = " + result.ToString()));
        }

        /// <summary>
        /// Test addition operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionAdditionOperator()
        {
            Quaternion leftSide = new Quaternion(1.0, 2.0, 3.0, 4.0);
            Quaternion rightSide = new Quaternion(1.0, 2.0, 3.0, 4.0);
            Quaternion result = leftSide + rightSide;
            Assert.IsTrue(
                Math.Abs(result.W - 8.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.X - 2.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 4.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 6.0) < MathConstants.ErrorEpsilon,
                string.Format("Quaternion addition failed result = " + result.ToString()));
        }

        /// <summary>
        /// Tests the quaternion length method
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionLength()
        {
            Quaternion quat = new Quaternion(2.0, 2.0, 2.0, 2.0);
            double length = Quaternion.Length(quat);
            Assert.IsTrue(
                Math.Abs(length - 4.0) < MathConstants.ErrorEpsilon, 
                "Quaternion length failed length = " + length.ToString());
        }

        /// <summary>
        /// Tests the quaternion normalize method
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionNormalize()
        {
            Quaternion quat = new Quaternion(2.0, 2.0, 2.0, 2.0);
            double length = Quaternion.Length(Quaternion.Normalize(quat));
            Assert.IsTrue(
                Math.Abs(length - 1.0) < MathConstants.ErrorEpsilon, 
                "Quaternion normalize failed length = " + length.ToString());
            quat = new Quaternion(0.0, 0.0, 0.0, 0.0);
            Quaternion result = Quaternion.Normalize(quat);
            Assert.IsTrue(
                Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon, 
                "Quaternion of zero normalize failed " + result.ToString());
        }

        /// <summary>
        /// Test the ToString operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionToString()
        {
            Quaternion quat = new Quaternion(1.0, 2.0, 3.0, 4.0);
            string str = quat.ToString();

            // Not sure what the test should be
            Assert.IsTrue(str != null, "Quaternion tostring failed");
        }

        /// <summary>
        /// Test the Rotate operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionRotate()
        {
            double angle = Math.PI / 2.0;
            Vector3 xaxisVector = new Vector3(Math.Sin(angle / 2.0) * 1.0, 0.0, 0.0);
            Vector3 yaxisVector = new Vector3(0.0, Math.Sin(angle / 2.0) * 1.0, 0.0);
            Vector3 zaxisVector = new Vector3(0.0, 0.0, Math.Sin(angle / 2.0) * 1.0);

            Quaternion quat = new Quaternion(xaxisVector.X, xaxisVector.Y, xaxisVector.Z, Math.Cos(angle / 2.0));
            Vector3 vector2Rotate = new Vector3(0.0, 1.0, 0.0);

            Vector3 result = Quaternion.Rotate(quat, vector2Rotate);
            Assert.IsTrue(
                Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 1.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion rotate Y around X failed result = " + result.ToString()));

            vector2Rotate.Y = 0.0;
            vector2Rotate.Z = 1.0;

            result = Quaternion.Rotate(quat, vector2Rotate);
            Assert.IsTrue(
                Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y + 1.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion rotate Z around X failed result = " + result.ToString()));

            quat.X = yaxisVector.X;
            quat.Y = yaxisVector.Y;
            quat.Z = yaxisVector.Z;
            vector2Rotate.Y = 0.0;
            vector2Rotate.Z = 1.0;

            result = Quaternion.Rotate(quat, vector2Rotate);
            Assert.IsTrue(
                Math.Abs(result.X - 1.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion rotate Z around Y failed result = " + result.ToString()));

            vector2Rotate.Z = 0.0;
            vector2Rotate.X = 1.0;

            result = Quaternion.Rotate(quat, vector2Rotate);
            Assert.IsTrue(
                Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z + 1.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion rotate X around Y failed result = " + result.ToString()));

            quat.X = zaxisVector.X;
            quat.Y = zaxisVector.Y;
            quat.Z = zaxisVector.Z;

            result = Quaternion.Rotate(quat, vector2Rotate);
            Assert.IsTrue(
                Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 1.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion rotate X around Z failed result = " + result.ToString()));

            vector2Rotate.X = 0.0;
            vector2Rotate.Y = 1.0;

            result = Quaternion.Rotate(quat, vector2Rotate);
            Assert.IsTrue(
                Math.Abs(result.X + 1.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon, 
                string.Format("Quaternion rotate Y around Z failed result = " + result.ToString()));
        }

        /// <summary>
        /// Tests the Hashcode method
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionGetHashCode()
        {
            Quaternion quat = new Quaternion();
            int result = quat.GetHashCode();
            Assert.IsTrue(result != 0, "Quaternion hash code is zero");
        }

        /// <summary>
        /// Tests the overridden equals method
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionEqualityMethod()
        {
            Quaternion leftSide = new Quaternion(1.0, 2.0, 3.0, 4.0);
            Quaternion rightSide = new Quaternion(leftSide);
            Assert.IsTrue(
                leftSide.Equals(rightSide), 
                "Quaternion equals method failed Left Side = " + leftSide.ToString() + " Right Side = "
                + rightSide.ToString());
            Assert.IsFalse(
                leftSide.Equals(new Vector3()), "Quaternion equals method should return false for disimular objects");
            leftSide.X = 5.0;
            Assert.IsFalse(
                leftSide.Equals(rightSide), 
                "Quaternion should not be equal in X component Left Side = " + leftSide.ToString() + " Right Side = "
                + rightSide.ToString());
            leftSide.X = 1.0;
            leftSide.Y = 5.0;
            Assert.IsFalse(
                leftSide.Equals(rightSide), 
                "Quaternion should not be equal in Y component Left Side = " + leftSide.ToString() + " Right Side = "
                + rightSide.ToString());
            leftSide.Y = 2.0;
            leftSide.Z = 5.0;
            Assert.IsFalse(
                leftSide.Equals(rightSide), 
                "Quaternion should not be equal in Z component Left Side = " + leftSide.ToString() + " Right Side = "
                + rightSide.ToString());
            leftSide.Z = 3.0;
            leftSide.W = 5.0;
            Assert.IsFalse(
                leftSide.Equals(rightSide), 
                "Quaternion should not be equal in  component Left Side = " + leftSide.ToString() + " Right Side = "
                + rightSide.ToString());
        }

        /// <summary>
        /// Tests the equality operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionEqualityOperator()
        {
            Quaternion leftSide = new Quaternion(1.0, 2.0, 3.0, 4.0);
            Quaternion rightSide = new Quaternion(leftSide);
            Assert.IsTrue(leftSide == rightSide, "Quaternion should have been equal");
            rightSide.X = 5.0;
            Assert.IsFalse(
                leftSide == rightSide, 
                "Quaternion should have been not equal in X component Left side = " + leftSide.ToString()
                + " Right side = " + rightSide.ToString());
            rightSide.X = 1.0;
            rightSide.Y = 5.0;
            Assert.IsFalse(
                leftSide == rightSide, 
                "Quaternion should have been not equal in Y component Left side = " + leftSide.ToString()
                + " Right side = " + rightSide.ToString());
            rightSide.Y = 2.0;
            rightSide.Z = 5.0;
            Assert.IsFalse(
                leftSide == rightSide, 
                "Quaternion should have been not equal in Z component Left side = " + leftSide.ToString()
                + " Right side = " + rightSide.ToString());
            rightSide.Z = 3.0;
            rightSide.W = 5.0;
            Assert.IsFalse(
                leftSide == rightSide, 
                "Quaternion should have been not equal in W component Left side = " + leftSide.ToString()
                + " Right side = " + rightSide.ToString());
            Assert.IsTrue(
                leftSide != rightSide, 
                "Quaternion != operator should be true in W component Left side = " + leftSide.ToString()
                + " Right side = " + rightSide.ToString());
        }

        /// <summary>
        /// Tests the FromAxis methods
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionFromAxisAnagle()
        {
            Quaternion result = Quaternion.FromAxisAngle(new AxisAngle(new Vector3(1.0, 0.0, 0.0), Math.PI));
            Assert.IsTrue(
                Math.Abs(result.X - 1.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon, 
                "Quaternion FromAxis failed for X component result = " + result.ToString());

            result = Quaternion.FromAxisAngle(new AxisAngle(new Vector3(0.0, 1.0, 0.0), Math.PI));
            Assert.IsTrue(
                Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 1.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon, 
                "Quaternion FromAxis failed for Y component result = " + result.ToString());

            result = Quaternion.FromAxisAngle(new AxisAngle(new Vector3(0.0, 0.0, 1.0), Math.PI));
            Assert.IsTrue(
                Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 1.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon, 
                "Quaternion FromAxis failed for Z component result = " + result.ToString());

            result = Quaternion.FromAxisAngle(AxisAngle.CreateAxisAngleFromRotationalAxisNonUnitVector(new Vector3(2.0, 0.0, 0.0), Math.PI));
            Assert.IsTrue(
                Math.Abs(result.X - 1.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.W - 0.0) < MathConstants.ErrorEpsilon, 
                "Quaternion FromAxis failed for X non-normalized component result = " + result.ToString());
        }

        /// <summary>
        /// Tests ToAxis method
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionToAxis()
        {
            Quaternion quat = new Quaternion(1.0, 0.0, 0.0, 0.0);
            AxisAngle result = Quaternion.ToAxisAngle(quat);
            Assert.IsTrue(
                Math.Abs(result.Axis.X - 1.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Axis.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Axis.Z - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Angle - Math.PI) < MathConstants.ErrorEpsilon, 
                "Quaternion ToAxis failed X component result = " + result.ToString());
            quat.X = 0.0;
            quat.Y = 1.0;
            result = Quaternion.ToAxisAngle(quat);
            Assert.IsTrue(
                Math.Abs(result.Axis.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Axis.Y - 1.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Axis.Z - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Angle - Math.PI) < MathConstants.ErrorEpsilon, 
                "Quaternion ToAxis failed Y component result = " + result.ToString());
            quat.Y = 0.0;
            quat.Z = 1.0;
            result = Quaternion.ToAxisAngle(quat);
            Assert.IsTrue(
                Math.Abs(result.Axis.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Axis.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Axis.Z - 1.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Angle - Math.PI) < MathConstants.ErrorEpsilon, 
                "Quaternion ToAxis failed Z component result = " + result.ToString());
            quat.Z = 0.0;
            quat.W = 1.0;
            result = Quaternion.ToAxisAngle(quat);
            Assert.IsTrue(
                Math.Abs(result.Axis.X - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Axis.Y - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Axis.Z - 0.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Angle - 0.0) < MathConstants.ErrorEpsilon, 
                "Quaternion ToAxis failed W component result = " + result.ToString());
        }

        /// <summary>
        /// Tests creation from input output vectors
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void QuaternionFromVecs()
        {
            Vector3 x = new Vector3(1, 0, 0);
            Vector3 y = new Vector3(0, 1, 0);
            Vector3 z = new Vector3(0, 0, 1);
            Vector3 zm1 = new Vector3(0, 0, -1);
            Vector3 xy = new Vector3(-1, 1, 0.0);
            Vector3 xyz = new Vector3(1, -1, 0.5);
            Vector3 xyzm1 = new Vector3(-1, 1, -0.5);

            // Simple rotations about major axis
            Assert.IsTrue(QuaternionTest.CheckRotationFromVectors(z, y));

            // Null Rotation
            Assert.IsTrue(QuaternionTest.CheckRotationFromVectors(xy, xy));

            // Non major axis
            Assert.IsTrue(QuaternionTest.CheckRotationFromVectors(z, xy));

            // 180' rotation
            Assert.IsTrue(QuaternionTest.CheckRotationFromVectors(z, zm1));
            Assert.IsTrue(QuaternionTest.CheckRotationFromVectors(xyz, xyzm1));
        }

        /// <summary>
        /// Checks validity of rotation from input to output vector
        /// </summary>
        /// <param name="input">Input vector</param>
        /// <param name="output">Output vector</param>
        /// <returns>True if test passes</returns>
        private static bool CheckRotationFromVectors(Vector3 input, Vector3 output)
        {
            input = Vector3.Normalize(input);
            output = Vector3.Normalize(output);

            Quaternion q = Quaternion.QuaternionFromVectors(input, output);
            Vector3 r = Quaternion.Rotate(q, input);
            return IsVector3Equal(output, r);
        }

        /// <summary>
        /// Checks if two Vectors are numerically equal
        /// </summary>
        /// <param name="v1">Vector one</param>
        /// <param name="v2">Vector 2</param>
        /// <returns>True if each component differ by less than epsilon</returns>
        private static bool IsVector3Equal(Vector3 v1, Vector3 v2)
        {
            bool ret = false;

            if (Math.Abs(v1.X - v2.X) < MathConstants.ErrorEpsilon &&
                Math.Abs(v1.Y - v2.Y) < MathConstants.ErrorEpsilon &&
                Math.Abs(v1.Z - v2.Z) < MathConstants.ErrorEpsilon)
            {
                ret = true;
            }

            return ret;
        }
    }
}
